#include "QspiModel.h"

#include <stdexcept>

#include "xqspips.h"

#define QSPI_DEVICE_ID XPAR_XQSPIPS_0_DEVICE_ID

// 指令写入缓存,前面是指令和地址,后面是数据
static uint8_t WriteBuffer[1024 * 1024 * 20]{};

QspiDriver::QspiDriver(size_t flashSize, size_t sectorSize, size_t pageSize)
    : flashSize_(flashSize),
      sectorSize_(sectorSize),
      pageSize_(pageSize) {
  // 让buffer全是0xFF
  memset(WriteBuffer, 0xFF, sizeof(WriteBuffer));
  memset(ReadBuffer, 0xFF, sizeof(ReadBuffer));
  // 计算扇区数
  sectorCount_ = flashSize_ / sectorSize_;
  // 计算页面数
  pageCount_ = flashSize_ / pageSize_;

  this->initialize();
}

void QspiDriver::read(void *data, size_t size, uint32_t address) {
  // 检查地址是否越界
  if (address + size > flashSize_) {
    throw std::out_of_range("flash read address out of range");
  }

  // 需要跳过的数据,6是1个指令+4个地址字节+1个dummy
  static constexpr uint8_t skipSize{6};
  // 临时读取数据的容器
  static std::vector<uint8_t> tempReceiveBuffer(1024, 0);
  if (tempReceiveBuffer.size() < size + skipSize) {
    // 重新开辟
    tempReceiveBuffer.resize(size + skipSize);
  }

  WriteBuffer[COMMAND_OFFSET] = FAST_READ_CMD;

  WriteBuffer[ADDRESS_1_OFFSET] = address >> 24 & 0xFF;
  WriteBuffer[ADDRESS_2_OFFSET] = address >> 16 & 0xFF;
  WriteBuffer[ADDRESS_3_OFFSET] = address >> 8 & 0xFF;
  WriteBuffer[ADDRESS_4_OFFSET] = address & 0xFF;
  // 发送读取指令,6是1个指令+4个地址字节+1个dummy
  XQspiPs_PolledTransfer(&QspiInstance, WriteBuffer, tempReceiveBuffer.data(),
                         size + skipSize);
  auto dataPtr = tempReceiveBuffer.data();
  // 将读取到的数据拷贝到data中
  if (data != nullptr) {
    memcpy(data, tempReceiveBuffer.data() + skipSize, size);
  }
}

void QspiDriver::write(void *data, size_t size, uint32_t address) {
  // 检查地址是否越界
  if (address + size > flashSize_) {
    throw std::out_of_range("flash write address out of range");
  }

  // 先擦除
  this->sectorErased(address, size);

  // 写使能指令
  u8 WriteEnableCmd = {WRITE_ENABLE_CMD};
  XQspiPs_PolledTransfer(&QspiInstance, &WriteEnableCmd, NULL,
                         sizeof(WriteEnableCmd));

  // 指令,地址到FLASH中
  WriteBuffer[COMMAND_OFFSET] = WRITE_4_ADDR_CMD;
  WriteBuffer[ADDRESS_1_OFFSET] = address >> 24 & 0xFF;
  WriteBuffer[ADDRESS_2_OFFSET] = address >> 16 & 0xFF;
  WriteBuffer[ADDRESS_3_OFFSET] = address >> 8 & 0xFF;
  WriteBuffer[ADDRESS_4_OFFSET] = address & 0xFF;
  // 数据写入到WriteBuffer
  memcpy(&WriteBuffer[OVERHEAD_SIZE], data, size);

  // 发送写入指令
  auto r = XQspiPs_PolledTransfer(&QspiInstance, WriteBuffer, NULL,
                         size + OVERHEAD_SIZE);

  // 等待写入完成
  while (isFlashBusy()) {
  }
}

void QspiDriver::sectorErased(uint32_t address, size_t size) {
  // 当size超过整个flash的大小时,直接返回false
  if (size > (sectorCount_ * sectorSize_)) {
    throw std::out_of_range("Erase size exceeds flash size.");
  }

  // 写使能
  u8 WriteEnableCmd = {WRITE_ENABLE_CMD};

  /*擦除的size恰好为整个flsah*/
  if (size == (sectorCount_ * sectorSize_)) {
    // 先写使能
    XQspiPs_PolledTransfer(&QspiInstance, &WriteEnableCmd, NULL,
                           sizeof(WriteEnableCmd));

    // 整个flash擦除命令
    WriteBuffer[COMMAND_OFFSET] = BULK_ERASE_CMD;

    // 发送擦除命令
    XQspiPs_PolledTransfer(&QspiInstance, WriteBuffer, NULL, BULK_ERASE_SIZE);

    // 等待擦除完成
    while (isFlashBusy()) {
    }
  }

  /*擦除的size<flash的size*/
  // 擦除的扇区数
  auto total_sector = (size / sectorSize_) + 1;
  for (auto i = 0; i < total_sector; ++i) {
    // 写使能
    XQspiPs_PolledTransfer(&QspiInstance, &WriteEnableCmd, NULL,
                           sizeof(WriteEnableCmd));

    WriteBuffer[COMMAND_OFFSET] = SEC_ERASE_CMD;
    WriteBuffer[ADDRESS_1_OFFSET] = address >> 24 & 0xFF;
    WriteBuffer[ADDRESS_2_OFFSET] = address >> 16 & 0xFF;
    WriteBuffer[ADDRESS_3_OFFSET] = address >> 8 & 0xFF;
    WriteBuffer[ADDRESS_4_OFFSET] = address & 0xFF;

    // 发送扇区擦除命令
    XQspiPs_PolledTransfer(&QspiInstance, WriteBuffer, NULL, SEC_ERASE_SIZE);

    // 等待擦除完毕
    while (isFlashBusy()) {
    }

    address += sectorSize_;
  }
}

void QspiDriver::initialize() {
  // 初始化QSPI驱动
  auto QspiConfig = XQspiPs_LookupConfig(QSPI_DEVICE_ID);
  if (QspiConfig == NULL) {
    throw std::runtime_error("QSPI config lookup failed");
  }

  auto Status =
      XQspiPs_CfgInitialize(&QspiInstance, QspiConfig, QspiConfig->BaseAddress);
  if (Status != XST_SUCCESS) {
    throw std::runtime_error("QSPI config initialize failed");
  }

  // 设置自动启动和手动片选模式
  XQspiPs_SetOptions(
      &QspiInstance,
      XQSPIPS_FORCE_SSELECT_OPTION | XQSPIPS_HOLD_B_DRIVE_OPTION);
  // 设置QSPI时钟的分频系数
  XQspiPs_SetClkPrescaler(&QspiInstance, XQSPIPS_CLK_PRESCALE_8);
  // 片选信号置为有效
  XQspiPs_SetSlaveSelect(&QspiInstance);
  // 初始化FLASH ID
  this->initID();
  // 使能4线模式
  this->initQuadType();
}

void QspiDriver::initID() {
  /* Read ID in Auto mode.*/
  // 此时还是3字节模式
  WriteBuffer[COMMAND_OFFSET] = READ_ID;
  /* 3 dummy bytes */
  WriteBuffer[ADDRESS_1_OFFSET] = 0x23;
  WriteBuffer[ADDRESS_2_OFFSET] = 0x08;
  WriteBuffer[ADDRESS_3_OFFSET] = 0x09;

  // 读取到的数据中,index1是厂商的标志,index2/3是flash大小的标志
  auto Status = XQspiPs_PolledTransfer(&QspiInstance, WriteBuffer, ReadBuffer,
                                       RD_ID_SIZE);
  if (Status != XST_SUCCESS) {
    throw std::runtime_error("QSPI read id failed");
  }

  printf("FlashID=0x%x 0x%x 0x%x\n\r", ReadBuffer[1], ReadBuffer[2],
         ReadBuffer[3]);
}

void QspiDriver::initQuadType() {
  WriteBuffer[COMMAND_OFFSET] = Enter4ByteAddress;

  // 发送进入4字节地址模式的命令
  auto Status = XQspiPs_PolledTransfer(&QspiInstance, WriteBuffer, nullptr, 1);
  if (Status != XST_SUCCESS) {
    throw std::runtime_error("QSPI Enter 4-Byte Address failed");
  }
}

bool QspiDriver::isFlashBusy() {
  // 检查FLASH状态寄存器
  static u8 ReadStatusCmd[] = {READ_STATUS_CMD, 0}; /* must send 2 bytes */
  static u8 FlashStatus[2]{};
  XQspiPs_PolledTransfer(&QspiInstance, ReadStatusCmd, FlashStatus,
                         sizeof(ReadStatusCmd));
  // 当为0的时候进入空闲状态
  return (FlashStatus[1] & 0x01);
}
